escape = false

function OneTurnRight()
  StartingRotationB = rotationCounts(B)
  repeat
    forward(A, 20)
    forward(B, -15)
  until abs(rotationCounts(B)-StartingRotationB) > 100
  stop(A)
  stop(B)
end

function OneTurnLeft()
  StartingRotationA = rotationCounts(A)
  repeat
    forward(A, -15)
    forward(B, 20)
  until abs(rotationCounts(A)-StartingRotationA) > 100
  stop(A)
  stop(B)
end

function MoveAway(SonarDistance)
  StartSonarReading = sonarReading(4)
  OneTurnRight()
  if sonarReading(4) < SonarDistance then
    if sonarReading(4) > StartSonarReading then
      OneTurnRight()
    else
     OneTurnLeft()
     OneTurnLeft()
    end
  else
  end
end

function getHeading()
  return nxt.random(20, 30) 
end

function motion(HowFar, SonarDistance)
  StartingRotationA = rotationCounts(A)
  StartingRotationB = rotationCounts(B)
  forward(A, 50)
  forward(B, 50)
  if HowFar < (rotationCounts(A)-StartingRotationA) or HowFar < (rotationCounts(B)-StartingRotationB) then
     stop(A)
     stop(B)
  end
  
end

function turn(left, right, distance) 
    StartingRotationA = rotationCounts(A)
    StartingRotationB = rotationCounts(B)
    repeat
       write("Inside turn loop")
       forward(A, left)
       forward(B, right)
       if isPressed(ESCAPE) then
           escape = true
           break
       end
    until distance < abs(rotationCounts(A)-StartingRotationA) or distance < abs(rotationCounts(B)-StartingRotationB)
    write("Escaping turn loop")
end

function DirectedMotion(SonarDistance, FarToGo)
  if sonarReading(4) < SonarDistance then
     MoveAway(25)
  else
     escape = motion(FarToGo, SonarDistance)
  end
  if isPressed(ESCAPE) then
      escape = true
  end
end

function Wander()
  escape = false
  repeat
    write("Starting loop")
    resetRotationCount(A)
    resetRotationCount(B)
    sign = nxt.random(1, 3)
    left = nxt.random(10, 30)
    right = nxt.random(10, 30)
    if sign % 2 == 1 then
	left = left * -1
    else 
	right = right * -1
    end
    dist = nxt.random(100, 500)
    Far = nxt.random(300, 1000)
    repeat
        write("Trying to escape")
        escape = DirectedMotion(25, Far)
    until escape == true or rotationCounts(B) > Far
    write("Out of escape loop")
    turn(left, right, dist)
    write("End of main loop")
  until escape == true
  write("Totally done")
  stop(A)
  stop(B)
end




